#include <hidef.h>         /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#include <MC9S12XS128.h>

#include  "system.h"
#include  "define.h"
#include  "get_image.h"
#include  "image_deal.h"
#include  "HighContrl.h"
#include  "Moterctl.h"
#include  "OLED.h" 
#include  "key.h" 
Bool stopflag = 1; 
uchar Delaycnt = 0;
float distance = 0;    
int timeCount = 0;

float getTime()
{

  float time = 0;
  float kp = 0.008;
  if(g_Feedback_Rate != 0)
  timeCount ++;
  time = timeCount*kp;
  
  return time;
}
int getDecimal(float num)
{
  int temp;
  int decimal;
  
  temp = num*100;
  decimal = temp%100;
  
  return decimal;
}
void getDistance(void){
  
  float kp = 0.00532/5; //0.00532
  
  distance += kp*Absolute(g_Feedback_Rate);
  
}

                                 
void main(void) 
{            
    PLL_init ();
    SCI_init ();
    PT0_PT1_init();
    PWM_init();
    Parameter_init();
    LCD_Init();
  	EnableInterrupts;
  	
  	
 //   while(OutFlag==0) 
 //   {
 //     KEY();
  //  }   
    
    for(;;) 
   {
    LCD_P6x8Str(15,1,"distance:");
    LCD_num(70,1,distance);
    LCD_P6x8Str(90,1,".");
    LCD_num(94,1,getDecimal(distance));
    LCD_P6x8Str(120,1,"m");
    LCD_P6x8Str(15,2,"Time:");
    LCD_num(70,2,getTime());
    LCD_P6x8Str(120,2,"s");
       if( over_flag ) 
       {
         // Extract_midpoint(); 
        //  Get_midpoint();
         New_Imige_deal();
         High_Rise_Strage(); 
         //Get_PC_Rate();          
         Carry_out_speed(50); 
        //  Edge_Mid_send();
          getDistance();
          over_flag = 0; 
       }  
   } 
}








